//GeeKee CeeBee
// ************ DEFINITIONS************
#include <Arduino.h>
float kp = 0.005;
float ki = 0.0 ;
float kd = 0.0001;

unsigned long t;
unsigned long t_prev = 0;

const byte interruptPinA = 2;
const byte interruptPinB = 3;
volatile long EncoderCount = 0;
const byte PWMPin = 6;
const byte DirPin1 = 7;
const byte DirPin2 = 8;

volatile unsigned long count = 0;
unsigned long count_prev = 0;

float Theta, RPM, RPM_d;
float Theta_prev = 0;
int dt;
float RPM_max = 230;

#define pi 3.1416
float Vmax = 6;
float Vmin = -6;
float V = 0.1;
float e, e_prev = 0, inte, inte_prev = 0;

//**********FUNCTIONS******************
//     Void ISR_EncoderA
//     Void ISR_EncoderB
//     Void Motor Driver Write
//     Timer Interrupt
//*************************************
void ISR_EncoderA() {
  bool PinB = digitalRead(interruptPinB);
  bool PinA = digitalRead(interruptPinA);

  if (PinB == LOW) {
    if (PinA == HIGH) {
      EncoderCount++;
    }
    else {
      EncoderCount--;
    }
  }

  else {
    if (PinA == HIGH) {
      EncoderCount--;
    }
    else {
      EncoderCount++;
    }
  }
}



void ISR_EncoderB() {
  bool PinB = digitalRead(interruptPinA);
  bool PinA = digitalRead(interruptPinB);

  if (PinA == LOW) {
    if (PinB == HIGH) {
      EncoderCount--;
    }
    else {
      EncoderCount++;
    }
  }

  else {
    if (PinB == HIGH) {
      EncoderCount++;
    }
    else {
      EncoderCount--;
    }
  }
}


float sign(float x) {
  if (x > 0) {
    return 1;
  } else if (x < 0) {
    return -1;
  } else {
    return 0;
  }
}

//***Motor Driver Functions*****

void WriteDriverVoltage(float V, float Vmax) {
  int PWMval = int(255 * abs(V) / Vmax);
  if (PWMval > 255) {
    PWMval = 255;
  }
  if (V > 0) {
    digitalWrite(DirPin1, HIGH);
    digitalWrite(DirPin2, LOW);
  }
  else if (V < 0) {
    digitalWrite(DirPin1, LOW);
    digitalWrite(DirPin2, HIGH);
  }
  else {
    digitalWrite(DirPin1, LOW);
    digitalWrite(DirPin2, LOW);
  }
  analogWrite(PWMPin, PWMval);
}

void setup() {
  Serial.begin(9600);
  pinMode(interruptPinA, INPUT_PULLUP);
  pinMode(interruptPinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPinA), ISR_EncoderA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(interruptPinB), ISR_EncoderB, CHANGE);
  pinMode(DirPin1, OUTPUT);
  pinMode(DirPin2, OUTPUT);

  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = 12499; //Prescaler = 64
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS11 | 1 << CS10);
  TIMSK1 |= (1 << OCIE1A);
  sei();
}

void loop() {
  if (count > count_prev) {
    t = millis();
    Theta = EncoderCount / 900.0;
    dt = (t - t_prev);
    RPM_d = RPM_max * (sin(2 * pi * 0.005 * t / 1000.0))
        * sign(sin(2 * pi * 0.05 * t / 1000.0));
    if (t / 1000.0 > 100) {
      RPM_d = 0;
    }
    RPM = (Theta - Theta_prev) / (dt / 1000.0) * 60;
    e = RPM_d - RPM;
    inte = inte_prev + (dt * (e + e_prev) / 2);
    V = kp * e + ki * inte + (kd * (e - e_prev) / dt) ;
    if (V > Vmax) {
      V = Vmax;
      inte = inte_prev;
    }
    if (V < Vmin) {
      V = Vmin;
      inte = inte_prev;
    }


    WriteDriverVoltage(V, Vmax);

    Serial.print(RPM_d); Serial.print(" \t");
    Serial.print(RPM); Serial.print(" \t ");
    Serial.print(V); Serial.print("\t  ");
    Serial.print(e); Serial.println("  ");

    Theta_prev = Theta;
    count_prev = count;
    t_prev = t;
    inte_prev = inte;
    e_prev = e;
  }

}



ISR(TIMER1_COMPA_vect) {
  count++;
  Serial.print(count * 0.05); Serial.print(" \t");
}

// #include <PS2X_lib.h>
// #include "pins.h"
// #include <Encoder.h> // 引入编码器库（Arduino IDE内置）

// PS2X ps2x; // 创建PS2手柄实例
// int error = 0;
// byte type = 0;
// byte vibrate = 0;

// class MotorController {
//     private:
//     int right_pin;
//     int left_pin;
//     int sensor_pin_A;
//     int sensor_pin_B;
   
//     public:
//     MotorController(int right_pin, int left_pin,int sensor_pin_A) {
//         this->right_pin = right_pin;
//         this->left_pin = left_pin;
//         this->sensor_pin_A = sensor_pin_A;
//         this->type="half_encoder"
//     }

//     void init(){
//         pinMode(right_pin, OUTPUT);
//         pinMode(left_pin, OUTPUT);
//     }
//     void setSpeed(int speed) {
//         if(speed > 0){
//             analogWrite(right_pin, speed);
//             analogWrite(left_pin, 0);
//         }else{
//             analogWrite(right_pin, 0);
//             analogWrite(left_pin, -speed);
//         }
//     }

// };


// MotorController  motorController(MOTOR2_DIR_POS, MOTOR2_DIR_NEG);
// void setup() {
//     Serial.begin(9600);
//     motorController.init();
//     // 初始化PS2手柄
//     // delay(300); // 等待控制器初始化
//     // error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, PRESSURE, RUMBLE);

//     // if (error == 0) {
//     // Serial.println("Found Controller, configured successfully");
//     // } else {
//     // Serial.println("Error connecting to PS2 controller");
//     // }
//     // type = ps2x.readType();
//     // switch (type) {
//     // case 0:
//     //     Serial.println("Unknown Controller type");
//     //     break;
//     // case 1:
//     //     Serial.println("DualShock Controller Found");
//     //     break;
//     // }
    
// }

// int leftStickX_pre=0;
// int leftStickY_pre=0;
// int rightStickX_pre=0;
// int rightStickY_pre=0;

// struct JoystickData {
//     int LX;
//     int LY;
//     int RX;
//     int RY;
// };

// JoystickData PS2_ReadJoystick() {
//     ps2x.read_gamepad(false, vibrate);
//     JoystickData data;
//     data.LX = ps2x.Analog(PSS_LX);
//     data.LY = ps2x.Analog(PSS_LY);
//     data.RX = ps2x.Analog(PSS_RX);
//     data.RY = ps2x.Analog(PSS_RY);

//     return data;
// }

// void loop() {
//     if (error == 1) return; // 如果有错误，跳过循环
//     JoystickData joy = PS2_ReadJoystick();
//     digitalWrite(MOTOR1_DIR_POS, LOW);
//     digitalWrite(MOTOR1_DIR_NEG, HIGH);
//     // analogWrite(MOTOR_A_EN, HIGH);
//     // Serial.print("Left Stick: X=");
//     // Serial.print(joy.LX);
//     // Serial.print(" Y=");
//     // Serial.println(joy.LY);
//     for (int speed =0; speed>=-200; speed-=10){
//         motorController.setSpeed(speed);
//         Serial.println(speed);
//         delay(1000-abs(speed));

//     }
//     for (int speed=-200; speed<=200; speed+=10){
//         motorController.setSpeed(speed);
//         Serial.println(speed);
//         delay(1000-abs(speed));
//     }
//     for(int speed=200; speed>=0; speed-=10){
//         motorController.setSpeed(speed);
//         Serial.println(speed);
//         delay(1000-abs(speed));

//     }
    
    
// //   Serial.print("Right Stick: X=");
// //   Serial.print(rightStickX);
// //   Serial.print(" Y=");
// //   Serial.println(rightStickY);

// //   if (upPressed || rightPressed || downPressed || leftPressed) {
// //     Serial.print("D-Pad: ");
// //     if (upPressed)    Serial.print("UP ");
// //     if (rightPressed) Serial.print("RIGHT ");
// //     if (downPressed)  Serial.print("DOWN ");
// //     if (leftPressed)  Serial.print("LEFT ");
// //     Serial.println();
// //   }

// //   // 检测按键是否按下
// //   if (ps2x.Button(PSB_CROSS))     Serial.println("Cross button pressed");
// //   if (ps2x.Button(PSB_CIRCLE))    Serial.println("Circle button pressed");
// //   if (ps2x.Button(PSB_SQUARE))    Serial.println("Square button pressed");
// //   if (ps2x.Button(PSB_TRIANGLE))  Serial.println("Triangle button pressed");
// //   if (ps2x.Button(PSB_L1))        Serial.println("L1 button pressed");
// //   if (ps2x.Button(PSB_L2))        Serial.println("L2 button pressed");
// //   if (ps2x.Button(PSB_R1))        Serial.println("R1 button pressed");
// //   if (ps2x.Button(PSB_R2))        Serial.println("R2 button pressed");
// //   if (ps2x.Button(PSB_SELECT))    Serial.println("Select button pressed");
// //   if (ps2x.Button(PSB_START))     Serial.println("Start button pressed");

//   delay(100); // 控制刷新频率
// }